function rpc_model = fitting(vgcp_x, vgcp_y, vgcp_z, vgcp_col, vgcp_row, direction, varargin)
%hsRPC.fitting 使用 SVD + ICCV 拟合 RPC 模型
%
% Syntax:
%   rpc_model = hsRPC.fitting(vgcp_x, vgcp_y, vgcp_z ...
%                   , vgcp_col, vgcp_row ...
%                   , direction [...
%                   , 'RPCModel', model ...
%                   , "ImgSize", height_width);
%
% Params:
%   - vgcp_x       [required] [numeric] 虚拟控制点的物方坐标 经度
%   - vgcp_y       [required] [numeric] 虚拟控制点的物方坐标 纬度
%   - vgcp_z       [required] [numeric] 虚拟控制点的物方坐标 高度
%   - vgcp_col     [required] [numeric] 虚拟控制点的像方坐标 列(从0开始)
%   - vgcp_row     [required] [numeric] 虚拟控制点的像方坐标 行(从0开始)
%   - direction    [required] [choices] 拟合方向, 存在两个可选值
%       * 'backward'  由物方(x,y,z)转换为像方(c,r)
%       * 'forward'   由像方(c,r,z)转换为物方(x,y)
%   - RPCModel     [namevalue] [struct] 可直接指定RPC模型, 使用该模型缩放系数和偏移量
%   - ImgSize      [namevalue] [numeric; numel=2] 图像的高宽，计算像方坐标的缩放系数和偏移量  
%   - Method       [namevalue] [char; choices] 求解方程的方法
%       * 'SVD+ICCV' SVD最小二乘解 (double拟合推荐)
%       * 'LC+ICCV'  LC正则化最小二乘解 (single拟合推荐)
%   - K            [namevalue] [numeric; scalar] 待拟合点的归一化范围 [-K, K]
%
% Return:
%   - rpc_model 拟合得到的RPC模型
%
% Author: 
%   iam002, 2025年2月28日
%
% Update: 
%   iam002, 2025年3月10日
%
% Matlab Version: R2023a

    %% 解析 Name-Value
    % 输入解析器
    p = inputParser;

    % 可指定已有的RPCModel, 否则默认创建一个空的RPCModel
    addParameter(p, 'RPCModel', initRPCModel, @isRPCModel);

    % 是否输出详细信息
    addParameter(p, 'ImgSize', [], @isnumeric);

    % 方程求解方法
    addParameter(p, 'Method', 'SVD+ICCV', @ischar);
    
    % 待拟合点的归一化范围 [-K, K]
    addParameter(p, 'K', 1, @isnumeric);

    % 解析参数
    parse(p, varargin{:})

    rpc_model = p.Results.RPCModel;
    img_size = p.Results.ImgSize;
    method = p.Results.Method;
    K = p.Results.K;

    if isequal(direction, 'backward')
        is_backward = true;
    elseif isequal(direction, 'forward')
        is_backward = false;
    else
        error("invalid direction: %s\n", direction);
    end

    if isequal(method, 'SVD+ICCV')
        eqa_solve_func = @solveRPCcoeffs;
    elseif isequal(method, 'LC+ICCV')
        eqa_solve_func = @solveRPCCoeffsReg;
    else 
        error("invalid method: %s\n", method);
    end

    %% 计算RPC偏移和缩放系数
    if rpc_model.longScale == 1 && rpc_model.latScale == 1 ...
        && rpc_model.longOffset == 0 && rpc_model.latOffset == 0
        
        x_range = [min(vgcp_x), max(vgcp_x)];
        y_range = [min(vgcp_y), max(vgcp_y)];
        z_range = [min(vgcp_z), max(vgcp_z)];

        % 经度保留后6位小数, 便于存储为文本格式
        x_range(1) = floor(x_range(1) * 1000000) / 1000000;
        x_range(2) = ceil(x_range(2) * 1000000) / 1000000;

        % 纬度保留后6位小数, 便于存储为文本格式
        y_range(1) = floor(y_range(1) * 1000000) / 1000000;
        y_range(2) = ceil(y_range(2) * 1000000) / 1000000;

        % 高程向进行千位取整, 便于存储为文本格式
        z_range(1) = floor(z_range(1) / 1000) * 1000;
        z_range(2) = ceil(z_range(2) / 1000) * 1000;

        if isempty(img_size)
            col_range = [min(vgcp_col), max(vgcp_col)];
            row_range = [min(vgcp_row), max(vgcp_row)];
            
            % 保留后6位小数, 便于存储为文本格式
            col_range(1) = floor(col_range(1) * 1000000) / 1000000;
            col_range(2) = ceil(col_range(2) * 1000000) / 1000000;
            row_range(1) = floor(row_range(1) * 1000000) / 1000000;
            row_range(2) = ceil(row_range(2) * 1000000) / 1000000;
        else
            % NOTE 推荐在行列缩放和偏移系数中存放原图像的大小信息
            col_range = [0, img_size(2)];
            row_range = [0, img_size(1)];
            rpc_model.m_width = img_size(2);
            rpc_model.m_height = img_size(1);
        end

        rpc_model.m_direction = direction;

        if ~is_backward
            % 交换 
            tmp = x_range;
            x_range = col_range;
            col_range = tmp;

            tmp = y_range;
            y_range = row_range;
            row_range = tmp;

            clear tmp
        end

        rpc_model = initRPCOffset(rpc_model, x_range, y_range, z_range, col_range, row_range);
        rpc_model = initRPCScale(K, rpc_model, x_range, y_range, z_range, col_range, row_range);
    end

    if ~isequal(rpc_model.m_direction, 'unkown') && ~isequal(rpc_model.m_direction, direction)
        error("The direction of the RPC model is inconsistent!\n");
    end

    %% 
    if ~is_backward
        % 交换 
        tmp = vgcp_x;
        vgcp_x = vgcp_col;
        vgcp_col = tmp;

        tmp = vgcp_y;
        vgcp_y = vgcp_row;
        vgcp_row = tmp;
        
        clear tmp;
    end

    %% 正则化采样点 
    vgcp_col = (vgcp_col - rpc_model.sampOffset) / rpc_model.sampScale;
    vgcp_row = (vgcp_row - rpc_model.lineOffset) / rpc_model.lineScale;
    vgcp_x = (vgcp_x - rpc_model.longOffset) / rpc_model.longScale;
    vgcp_y = (vgcp_y - rpc_model.latOffset) / rpc_model.latScale;
    vgcp_z = (vgcp_z - rpc_model.heightOffset) / rpc_model.heightScale;
    
    
    %% 求解模型
    poly_mat = getPolyMat(vgcp_x, vgcp_y, vgcp_z);
    N = numel(vgcp_x);

    vec_samp = vgcp_col;
    mat_samp = [ones(N,1), poly_mat, -1*vec_samp.*poly_mat];
    
    vec_line = vgcp_row;
    mat_line = [ones(N,1), poly_mat, -1*vec_line.*poly_mat];
    
    tol = 1e-10;
    max_iter = 20;

    fprintf("[hsRPC.fitting] solve samp:\n");
    coeff_samp = eqa_solve_func(mat_samp, vec_samp, rpc_model.sampScale, tol, max_iter);
    rpc_model.sampNumCoef = coeff_samp(1:20);
    rpc_model.sampDenCoef = [1; coeff_samp(21:end)];

    fprintf("[hsRPC.fitting] solve line:\n");
    coeff_line = eqa_solve_func(mat_line, vec_line, rpc_model.lineScale, tol, max_iter);
    rpc_model.lineNumCoef = coeff_line(1:20);
    rpc_model.lineDenCoef = [1; coeff_line(21:end)];

end

%%
function res = isRPCModel(rpc_model)
    if ~isstruct(rpc_model)
        res = false;
        return;
    end
    
    if ~isfield(rpc_model, 'SpecId')
        res = false;
        return;
    end

    res = true;
end

function rpc_model = initRPCModel
    rpc_model = struct('m_className', 'RPC');
    rpc_model.m_filepath = '';
    rpc_model.m_direction = 'backward';
    rpc_model.m_width = -1;
    rpc_model.m_height = -1;
    rpc_model.satId = 'None';
    rpc_model.bandId = 'SAR';
    rpc_model.SpecId = 'RPC';
    rpc_model.errBias = 1;
    rpc_model.errRand = 0;
    rpc_model.sampOffset = 0;
    rpc_model.lineOffset = 0;
    rpc_model.latOffset = 0;
    rpc_model.longOffset = 0;
    rpc_model.heightOffset = 0;
    rpc_model.sampScale = 1;
    rpc_model.lineScale = 1;
    rpc_model.latScale = 1;
    rpc_model.longScale = 1;
    rpc_model.heightScale = 1;
    rpc_model.sampNumCoef = zeros(20, 1);
    rpc_model.sampDenCoef = zeros(20, 1);
    rpc_model.lineNumCoef = zeros(20, 1);
    rpc_model.lineDenCoef = zeros(20, 1);
end

function rpc_model = initRPCOffset(rpc_model, x_range, y_range, z_range, col_range, row_range)
    rpc_model.longOffset   = (x_range(1) + x_range(2)) / 2;
    rpc_model.latOffset    = (y_range(1) + y_range(2)) / 2;
    rpc_model.heightOffset = (z_range(1) + z_range(2)) / 2;
    rpc_model.sampOffset   = (col_range(1) + col_range(2)) / 2;
    rpc_model.lineOffset   = (row_range(1) + row_range(2)) / 2;

    rpc_model.longOffset = floor(rpc_model.longOffset * 1e6) / 1e6;
    rpc_model.latOffset = floor(rpc_model.latOffset * 1e6) / 1e6;
    rpc_model.heightOffset = floor(rpc_model.heightOffset * 1e6) / 1e6;
    rpc_model.sampOffset = floor(rpc_model.sampOffset * 1e6) / 1e6;
    rpc_model.lineOffset = floor(rpc_model.lineOffset * 1e6) / 1e6;
end

function rpc_model = initRPCScale(K, rpc_model, x_range, y_range, z_range, col_range, row_range)
    % K = 1;
    rpc_model.longScale   = (x_range(2) - x_range(1)) / 2 / K;
    rpc_model.latScale    = (y_range(2) - y_range(1)) / 2 / K;
    rpc_model.heightScale = (z_range(2) - z_range(1)) / 2 / K;
    rpc_model.sampScale   = (col_range(2) - col_range(1)) / 2 / K;
    rpc_model.lineScale   = (row_range(2) - row_range(1)) / 2 / K;

    rpc_model.longScale = floor(rpc_model.longScale * 1e6) / 1e6;
    rpc_model.latScale = floor(rpc_model.latScale * 1e6) / 1e6;
    rpc_model.heightScale = floor(rpc_model.heightScale * 1e6) / 1e6;
    rpc_model.sampScale = floor(rpc_model.sampScale * 1e6) / 1e6;
    rpc_model.lineScale = floor(rpc_model.lineScale * 1e6) / 1e6;
end

function poly_mat = getPolyMat(L, P, H)
    poly_mat = [      L,      P,      H ...
               ,   L.*P,   L.*H,   P.*H ...
               ,   L.*L,   P.*P,   H.*H ...
               , P.*L.*H, L.*L.*L, L.*P.*P, L.*H.*H, L.*L.*P ...
               , P.*P.*P, P.*H.*H, L.*L.*H, P.*P.*H, H.*H.*H];
end

function y = computeRPCrmse(A, b, x, scale)
    y = A*x - b;
    y = scale.*y;
    y = sqrt( mean(y.^2) );
end

function x_best = solveRPCcoeffs(A, b, scale, tol, max_iter)
    
    [U, S, V] = svd(A, "econ");
    iS = S;
    iS(1 == eye(size(iS))) = 1.0 ./ diag(S);
    x = V*iS*U'*b;
    
    Id = eye(numel(x));
    S2I = S.^2 + Id;
    inv_S2I = S2I;
    inv_S2I(1 == eye(size(iS))) = 1.0 ./ diag(S2I);
    
    M = V * inv_S2I * S * U' * b;
    K = V * inv_S2I * V';
    
    rmse = computeRPCrmse(A, b, x, scale);
    x_best = x;
    rmse_best = rmse;

    fprintf("[%02d] rmse: %16.12f\n", 0, rmse_best);
    
    for i = 1:max_iter
        x = M + K*x;

        rmse_prev = rmse;
        rmse = computeRPCrmse(A, b, x, scale);

        if rmse < rmse_best
            x_best = x;
            rmse_best = rmse;
        end
        
        fprintf("[%02d] rmse: %16.12f\n", i, rmse);

        if abs(rmse_prev - rmse) < tol
            break;
        end
    end
end

function x_best = solveRPCCoeffsReg(A, b, scale, tol, max_iter)

    % 通过 L-curve 曲线找到最优的正则项系数, 并求得正则最小二乘解
    [lam, x, rho, eta] = lcurve(A, b, 'Show', false);
    fprintf("Reg lam: %.10f, rho: %.10f, eta: %.10f\n", lam, rho, eta);

    % 后续步骤同 solveRPCcoeffs
    [U, S, V] = svd(A, "econ");
    iS = S;
    iS(1 == eye(size(iS))) = 1.0 ./ diag(S);
    Id = eye(numel(x));
    S2I = S.^2 + Id;
    inv_S2I = S2I;
    inv_S2I(1 == eye(size(iS))) = 1.0 ./ diag(S2I);
    
    M = V * inv_S2I * S * U' * b;
    K = V * inv_S2I * V';

    rmse = computeRPCrmse(A, b, x, scale);
    x_best = x;
    rmse_best = rmse;

    fprintf("[%02d] rmse: %16.12f\n", 0, rmse_best);
    
    for i = 1:max_iter
        x = M + K*x;

        rmse_prev = rmse;
        rmse = computeRPCrmse(A, b, x, scale);

        if rmse < rmse_best
            x_best = x;
            rmse_best = rmse;
        end

        fprintf("[%02d] rmse: %16.12f\n", i, rmse);

        if abs(rmse_prev - rmse) < tol
            break;
        end
    end
end
